<!DOCTYPE html>

<html>

  <head>
    <title>Underactuated Robotics: Motion Planning as
Search</title>
    <meta name="Underactuated Robotics: Motion Planning as
Search" content="text/html; charset=utf-8;" />
    <link rel="canonical" href="http://underactuated.mit.edu/planning.html" />

    <script src="https://hypothes.is/embed.js" async></script>
    <script type="text/javascript" src="htmlbook/book.js"></script>

    <script src="htmlbook/mathjax-config.js" defer></script> 
    <script type="text/javascript" id="MathJax-script" defer
      src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-chtml.js">
    </script>
    <script>window.MathJax || document.write('<script type="text/javascript" src="htmlbook/MathJax/es5/tex-chtml.js" defer><\/script>')</script>

    <link rel="stylesheet" href="htmlbook/highlight/styles/default.css">
    <script src="htmlbook/highlight/highlight.pack.js"></script> <!-- http://highlightjs.readthedocs.io/en/latest/css-classes-reference.html#language-names-and-aliases -->
    <script>hljs.initHighlightingOnLoad();</script>

    <link rel="stylesheet" type="text/css" href="htmlbook/book.css" />
  </head>

<body onload="loadChapter('underactuated');">

<div data-type="titlepage">
  <header>
    <h1><a href="index.html" style="text-decoration:none;">Underactuated Robotics</a></h1>
    <p data-type="subtitle">Algorithms for Walking, Running, Swimming, Flying, and Manipulation</p> 
    <p style="font-size: 18px;"><a href="http://people.csail.mit.edu/russt/">Russ Tedrake</a></p>
    <p style="font-size: 14px; text-align: right;"> 
      &copy; Russ Tedrake, 2020<br/>
      <a href="tocite.html">How to cite these notes</a> &nbsp; | &nbsp;
      <a target="_blank" href="https://docs.google.com/forms/d/e/1FAIpQLSesAhROfLRfexrRFebHWLtRpjhqtb8k_iEagWMkvc7xau08iQ/viewform?usp=sf_link">Send me your feedback</a><br/>
    </p>
  </header>
</div>

<p><b>Note:</b> These are working notes used for <a
href="http://underactuated.csail.mit.edu/Spring2020/">a course being taught
at MIT</a>. They will be updated throughout the Spring 2020 semester.  <a 
href="https://www.youtube.com/channel/UChfUOAhz7ynELF-s_1LPpWg">Lecture  videos are available on YouTube</a>.</p> 

<table style="width:100%;"><tr style="width:100%">
  <td style="width:33%;text-align:left;"><a class="previous_chapter" href=trajopt.html>Previous Chapter</a></td>
  <td style="width:33%;text-align:center;"><a href=index.html>Table of contents</a></td>
  <td style="width:33%;text-align:right;"><a class="next_chapter" href=feedback_motion_planning.html>Next Chapter</a></td>
</tr></table>


<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 10"><h1>Motion Planning as
Search</h1>

  <p>The term "motion planning" is a hopelessly general term which almost
  certainly encompasses the dynamic programming, feedback design, and
  trajectory optimization algorithms that we have already discussed.  However,
  there are a number of algorithms and ideas that we have not yet discussed
  which have grown from the idea of formulating motion planning as a search
  problem -- for instance searching for a path from a start to a goal in a
  graph which is too large solve completely with dynamic programming. Some, but
  certainly not all, of these algorithms sacrifice optimality in order to find
  any path if it exists, and the notion of a planner being "complete" --
  guaranteed to find a path if one exists -- is highly valued.  This is
  precisely our goal for this chapter, to add some additional tools that will
  be able to provide some form of solutions for our most geometrically complex,
  highly non-convex, robot control problems.</p>

  <p><elib>LaValle06</elib> is a very nice book on planning algorithms in
  general and on motion planning algorithms in particular.  Compared to other
  planning problems, <em>motion planning</em> typically refers to problems
  where the planning domain is continuous (e.g. continuous state space,
  continuous action space), but many motion planning algorithms trace their
  origins back to ideas in discrete domains (e.g., graph search).</p>

  <p>For this chapter, we will consider the following problem formulation:
  given a system defined by the nonlinear dynamics (in continuous- or
  discrete-time) $$\dot{\bx} = f(\bx,\bu) \quad \text{or} \quad \bx[n+1] =
  f(\bx[n],\bu[n]),$$ and given a start state $\bx(0) = \bx_0$ and a goal
  region ${\cal G}$, find any finite-time trajectory from $\bx_0$ to to $\bx
  \in {\cal G}$ if such a trajectory exists.</p>

  <section><h1>Artificial Intelligence as Search</h1>

    <p>A long history...  some people feel that the route to creating
    intelligent machines is to collect large ontologies of knowledge, and then
    perform very efficient search. (The more modern view of AI is focused
    instead on machine learning, the right answer probably involves pieces of
    both.)  Samuels checker players, Deep Blue playing chess, theorem proving,
    Cyc, IBM Watson,...</p>

    <p>One of the key ideas is the use of "heuristics" to guide the search.
    "Is it possible to find and optimal path from the start to a goal without
    visiting every node?".  $A^*$.  Admissible heuristics. Example: google
    maps.</p>

    <p>Online planning.  $D^*$, $D^*$-Lite.</p>

  </section>

  <section><h1>Randomized motion planning</h1>

    <p>If you remember how we introduced dynamic programming initially as a
    graph search, you'll remember that there were some challenges in
    discretizing the state space.  Let's assume that we have discretized the
    continuous space into some finite set of discrete nodes in our graph.  Even
    if we are willing to discretize the action space for the robot (this might
    be even be acceptable in practice), we had a problem where discrete actions
    from one node in the graph, integrated over some finite interval $h$, are
    extremely unlikely to land exactly on top of another node in the graph.  To
    combat this, we had to start working on methods for interpolating the value
    function estimate between nodes.</p>

    <todo>add figure illustrating the interpolation here</todo>

    <p>Interpolation can work well if you are trying to solve for the
    cost-to-go function over the entire state space, but it's less compatible
    with search methods which are trying to find just a single path through the
    space.  If I start in node 1, and land between node 2 and node 3, then
    which node to I continue to expand from?</p>

    <p>One approach to avoiding this problem is to build a <em>search tree</em>
    as the search executes, instead of relying on a predefined mesh
    discretization.  This tree will contains nodes rooted in the continuous
    space at exactly the points where system can reach.</p>

    <p>Another other problem with any fixed mesh discretization of a continuous
    space, or even a fixed discretization of the action space, is that unless
    we have specific geometric / dynamic insights into our continuous system,
    it very difficult to provide a <em>complete</em> planning algorithm.  Even
    if we can show that no path to the goal exists on the tree/graph, how can
    we be certain that there is no path for the continuous system?  Perhaps a
    solution would have emerged if we had discretized the system differently,
    or more finely?</p>

    <p>One approach to addressing this second challenge is to toss out the
    notion of fixed discretizations, and replace them with random sampling
    (another approach would be to adaptively add resolution to the
    discretization as the algorithm runs).  Random sampling, e.g. of the action
    space, can yield algorithms that are <em>probabilistically complete</em>
    for the continuous space -- if a solution to the problem exists, then a
    probabilistically complete algorithm will find that solution with
    probability 1 as the number of samples goes to infinity.</p>

    <p>With these motivations in mind, we can build what is perhaps the
    simplest probabilistically complete algorithm for finding a path from the a
    starting state to some goal region with in a continuous state and action
    space:</p>

    <example><h1>Planning with a Random Tree</h1>

      <p>Let us denote the data structure which contains the tree as ${\cal T}$.  The algorithm is very simple:
      <ul>
        <li>Initialize the tree with the start state: ${\cal T} \leftarrow \bx_0$.</li>
        <li>On each iteration:
          <ul>
            <li>Select a random node, $\bx_{rand}$, from the tree, ${\cal T}$</li>
            <li>Select a random action, $\bu_{rand}$, from a distribution over feasible actions.</li>
            <li>Compute the dynamics: $\bx_{new} = f(\bx_{rand},\bu_{rand})$</li>
            <li>If $\bx_{new} \in {\cal G}$, then terminate. Solution found!</li>
            <li>Otherwise add the new node to the tree, ${\cal T} \leftarrow \bx_{new}$.</li>
          </ul>
        </li>
      </ul>
      It can be shown that this algorithm is, in fact, probabilistically complete.  However,
      without strong heuristics to guide the selection of the nodes scheduled for expansion,
      it can be extremely inefficient.  For a simple example, consider the system $\bx[n] = \bu[n]$ with $\bx \in \Re^2$ and $\bu_i \in [-1,1]$.
      We'll start at the origin and put the goal region as $\forall i, 15 \le x_i \le 20$.
      Try it yourself:

      <pre><code class="matlab" testfile="testRandomTree">
T = struct('parent',zeros(1,1000),'node',zeros(2,1000));  % pre-allocate memory for the "tree"
for i=2:size(T.parent,2)
T.parent(i) = randi(i-1);
x_rand = T.node(:,T.parent(i));
u_rand = 2*rand(2,1)-1;
x_new = x_rand+u_rand;
if (15<=x_new(1) && x_new(1)<=20 && 15<=x_new(2) && x_new(2)<=20)
  disp('Success!'); break;
end
T.node(:,i) = x_new;
end
clf;
line([T.node(1,T.parent(2:end));T.node(1,2:end)],[T.node(2,T.parent(2:end));T.node(2,2:end)],'Color','k');
patch([15,15,20,20],[15,20,20,15],'g')
axis([-10,25,-10,25]);
</code></pre>
      Again, this algorithm <em>is</em> probabilistically complete.  But after expanding 1000 nodes, the tree is basically a mess of node points all right on top of
      each other:
      <figure>
        <img width="70%" src="figures/random_tree.svg"/>
      </figure>
      <p>We're nowhere close to the goal yet, and it's not exactly a hard problem.
      </p>

    </example>

    <p>
      While the idea of generating a tree of feasible points has clear advantages, we have lost
      the ability to cross off a node (and therefore a region of space) once it has been explored.
      It seems that, to make randomized algorithms effective, we are going to at the very least need some
      form of heuristic for encouraging the nodes to spread out and explore the space.
    </p>

    <subsection><h1>Rapidly-Exploring Random Trees (RRTs)</h1>

      <figure>
        <img width="70%" src="figures/rrt.svg"/>
      </figure>

      <figure>
        <img width="90%" src="figures/rrt_basic.svg"/>
        <figcaption>(<a href="figures/rrt_basic.swf">Click here to watch the animation</a>)</figcaption>
      </figure>

      <figure>
        <img width="90%" src="figures/rrt_voronoi.svg"/>
        <figcaption>(<a href="figures/rrt_voronoi.swf">Click here to watch the animation</a>)</figcaption>
      </figure>


    </subsection>

    <subsection><h1>RRTs for robots with dynamics</h1>

    </subsection>

    <subsection><h1>Variations and extensions</h1>

      <p>Multi-query planning with PRMs, ...
      </p>

      <p>RRT*, RRT-sharp, RRTx, ...  </p>

      <p>Kinodynamic-RRT*, LQR-RRT(*)</p>

      <p>Complexity bounds and dispersion limits</p>

    </subsection>

    <subsection><h1>Discussion</h1>

      <p>Not sure yet whether randomness is fundamental here, or whether is a temporary "crutch"
        until we understand geometric and dynamic planning better.</p>

    </subsection>
  </section>

  <section><h1>Decomposition methods</h1>

    <p>Cell decomposition...</p>

    <p>Mixed-integer planning.</p>

    <p>Approximate decompositions for complex environments (e.g. IRIS)</p>

  </section>

  <section><h1>Exercises</h1>

    <exercise><h1>RRT Planning</h1>

      <p>In this <a href="https://colab.research.google.com/github/RussTedrake/underactuated/blob/master/exercises/planning/rrt_planning/rrt_planning.ipynb" target="_blank">notebook</a> 
         we will write code for the Rapidly-Exploring Random 
         Tree (RRT). Building on this implementation we will also implement RRT*, 
         a variant of RRT that converges towards an optimal solution.</p>

      <ol type="a">

        <li>Implement RRT</li>

        <li>Implement RRT*</li>

      </ol>

    </exercise>


  </section>


</chapter>
<!-- EVERYTHING BELOW THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->

<table style="width:100%;"><tr style="width:100%">
  <td style="width:33%;text-align:left;"><a class="previous_chapter" href=trajopt.html>Previous Chapter</a></td>
  <td style="width:33%;text-align:center;"><a href=index.html>Table of contents</a></td>
  <td style="width:33%;text-align:right;"><a class="next_chapter" href=feedback_motion_planning.html>Next Chapter</a></td>
</tr></table>

<div id="footer">
  <hr>
  <table style="width:100%;">
    <tr><td><em>Underactuated Robotics</em></td><td align="right">&copy; Russ
      Tedrake, 2020</td></tr>
  </table>
</div>


</body>
</html>
